#define PWMB 5
#define BIN2 6
#define BIN1 7
#define stby 8
#define AIN1 9
#define AIN2 10
#define PWMA 11

#include <Wire.h>

void clockwiseA();
void clockwiseB();
void anticlockwiseA();
void anticlockwiseB();

struct
{
  double gx, gy, gz;
  double ax, ay, az;

  float Yaw, Pitch, Roll;  // 偏航角，俯仰角，翻滚角
  uint8_t speed = 0;

  void print() {
    Serial.print(" Pitch=");
    Serial.print(Pitch);
    Serial.print(" Roll=");
    Serial.println(Roll);
  }

  void forward() {
    clockwiseA();
    anticlockwiseB();
  }
  void backward() {
    anticlockwiseA();
    clockwiseB();
  }

  float q0 = 1, q1 = 0, q2 = 0, q3 = 0;   // 四元数的元素，代表估计方向
  float exInt = 0, eyInt = 0, ezInt = 0;  // 按比例缩小积分误差

#define Kp 100.0f     // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f     // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f  // 采样周期的一半
  void IMUupdate() {
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;

    // 测量正常化
    norm = sqrt(ax * ax + ay * ay + az * az);
    ax = ax / norm;  // 单位化
    ay = ay / norm;
    az = az / norm;

    // 估计方向的重力
    vx = 2 * (q1 * q3 - q0 * q2);
    vy = 2 * (q0 * q1 + q2 * q3);
    vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

    // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    // 积分误差比例积分增益
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    // 调整后的陀螺仪测量
    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;

    // 整合四元数率和正常化
    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

    // 正常化四元
    norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;

    this->Pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;                                 // pitch ,转换为度数
    this->Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;  // rollv                                                                          // Yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;  //此处没有价值，注掉
  }
  void update() {
    speed = abs(this->Pitch) / 90 * 230;
    if (speed > 255) {
      speed = 255;
    }
    if (abs(this->Pitch) < 10) {
      speed = 0;
    }
    analogWrite(PWMA, speed);
    analogWrite(PWMB, speed);
    if (this->Pitch > 0) {
      this->forward();
    } else {
      this->backward();
    }
  }
} MOVE;

void setup() {
  Wire.begin();
  Serial.begin(9600);
  setupMPU();
  setupTB6612();

  digitalWrite(stby, HIGH);
  MOVE.forward();
}

long loopCount = 0;
void loop() {

  if (loopCount++ % 40 == 0) {
    recordAccelRegisters();
    recordGyroRegisters();

    MOVE.IMUupdate();
    MOVE.update();

    Serial.print("speed:");
    Serial.print(MOVE.speed);
    MOVE.print();
  }
}

void setupMPU() {
  // REGISTER 0x6B/REGISTER 107:Power Management 1
  Wire.beginTransmission(0b1101000);  // This is the I2C address of the MPU (b1101000/b1101001 for AC0 low/high datasheet Sec. 9.2)
  Wire.write(0x6B);                   // Accessing the register 6B/107 - Power Management (Sec. 4.30)
  Wire.write(0b00000000);             // Setting SLEEP register to 0, using the internal 8 Mhz oscillator
  Wire.endTransmission();

  // REGISTER 0x1b/REGISTER 27:Gyroscope Configuration
  Wire.beginTransmission(0b1101000);  // I2C address of the MPU
  Wire.write(0x1B);                   // Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
  Wire.write(0x00000000);             // Setting the gyro to full scale +/- 250deg./s (转化为rpm:250/360 * 60 = 41.67rpm) 最高可以转化为2000deg./s
  Wire.endTransmission();

  // REGISTER 0x1C/REGISTER 28:ACCELEROMETER CONFIGURATION
  Wire.beginTransmission(0b1101000);  // I2C address of the MPU
  Wire.write(0x1C);                   // Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
  Wire.write(0b00000000);             // Setting the accel to +/- 2g（if choose +/- 16g，the value would be 0b00011000）
  Wire.endTransmission();
}

void setupTB6612() {
  // put your setup code here, to run once:
  pinMode(BIN2, OUTPUT);  // 定义引脚的模式
  pinMode(BIN1, OUTPUT);  // 定义引脚的模式
  pinMode(PWMB, OUTPUT);  // 定义引脚的模式
  pinMode(stby, OUTPUT);  // 定义引脚的模式
  pinMode(AIN1, OUTPUT);  // 定义引脚的模式
  pinMode(AIN2, OUTPUT);  // 定义引脚的模式
  pinMode(PWMA, OUTPUT);  // 定义引脚的模式
}

long accelX, accelY, accelZ;
void recordAccelRegisters() {
  // REGISTER 0x3B~0x40/REGISTER 59~64
  Wire.beginTransmission(0b1101000);  // I2C address of the MPU
  Wire.write(0x3B);                   // Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6);  // Request Accel Registers (3B - 40)

  // 使用了左移<<和位运算|。Wire.read()一次读取1bytes，并在下一次调用时自动读取下一个地址的数据
  while (Wire.available() < 6)
    ;                                       // Waiting for all the 6 bytes data to be sent from the slave machine （必须等待所有数据存储到缓冲区后才能读取）
  accelX = Wire.read() << 8 | Wire.read();  // Store first two bytes into accelX （自动存储为定义的long型值）
  accelY = Wire.read() << 8 | Wire.read();  // Store middle two bytes into accelY
  accelZ = Wire.read() << 8 | Wire.read();  // Store last two bytes into accelZ

  // float = long / float
  MOVE.ax = accelX / 16384.0;
  MOVE.ay = accelY / 16384.0;
  MOVE.az = accelZ / 16384.0;
}

long gyroX, gyroY, gyroZ;
void recordGyroRegisters() {
  // REGISTER 0x43~0x48/REGISTER 67~72
  Wire.beginTransmission(0b1101000);  // I2C address of the MPU
  Wire.write(0x43);                   // Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6);  // Request Gyro Registers (43 ~ 48)
  while (Wire.available() < 6)
    ;
  gyroX = Wire.read() << 8 | Wire.read();  // Store first two bytes into accelX
  gyroY = Wire.read() << 8 | Wire.read();  // Store middle two bytes into accelY
  gyroZ = Wire.read() << 8 | Wire.read();  // Store last two bytes into accelZ'

  MOVE.gx = gyroX / 131.0;
  MOVE.gy = gyroY / 131.0;
  MOVE.gz = gyroZ / 131.0;
}

void clockwiseA() {
  digitalWrite(BIN2, HIGH);
  digitalWrite(BIN1, LOW);
  // Serial.println("A:shunshizhen");
}
void clockwiseB() {
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  // Serial.println("B:shunshizhen");
}
void anticlockwiseA() {
  digitalWrite(BIN2, LOW);
  digitalWrite(BIN1, HIGH);
  // Serial.println("A:nishizhen");
}
void anticlockwiseB() {
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, HIGH);
  // Serial.println("B:nishizhen");
}
void stopA() {
  digitalWrite(BIN2, LOW);
  digitalWrite(BIN1, LOW);
  // Serial.println("A:STOP");
}
void stopB() {
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  // Serial.println("B:STOP");
}